#include <vector>
#include "ceres/ceres.h"
#include "gflags/gflags.h"
#include "glog/logging.h"

using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::SizedCostFunction;
using ceres::Problem;
using ceres::Solve;
using ceres::Solver;

struct F1
{
    template<typename T>
    bool operator()(const T *const x1, const T *const x2, T *residual) const
    {
        // f1 = x1 + 10 * x2;
        residual[0] = x1[0] + 10.0 * x2[0];
        return true;
    }
};

struct F2
{
    template<typename T>
    bool operator()(const T *const x3, const T *const x4, T *residual) const
    {
        // f2 = sqrt(5) (x3 - x4)
        residual[0] = sqrt(5.0) * (x3[0] - x4[0]);
        return true;
    }
};

struct F3
{
    template<typename T>
    bool operator()(const T *const x2, const T *const x3, T *residual) const
    {
        // f3 = (x2 - 2 x3)^2
        residual[0] = (x2[0] - 2.0 * x3[0]) * (x2[0] - 2.0 * x3[0]);
        return true;
    }
};

struct F4
{
    template<typename T>
    bool operator()(const T *const x1, const T *const x4, T *residual) const
    {
        // f4 = sqrt(10) (x1 - x4)^2
        residual[0] = sqrt(10.0) * (x1[0] - x4[0]) * (x1[0] - x4[0]);
        return true;
    }
};
DEFINE_string(minimizer,
              "trust_region",
              "Minimizer type to use, choices are: line_search & trust_region");

class PowellAnalyticAll : public SizedCostFunction<4, 4>
{
public:
    PowellAnalyticAll()
    {}

    virtual ~PowellAnalyticAll()
    {}

    virtual bool Evaluate(double const *const *parameters,
                          double *residuals,
                          double **jacobians) const
    {
        const double x1 = parameters[0][0];
        const double x2 = parameters[0][1];
        const double x3 = parameters[0][2];
        const double x4 = parameters[0][3];
        residuals[0] = x1 + 10.0 * x2;
        residuals[1] = sqrt(5.0) * (x3 - x4);
        residuals[2] = (x2 - 2.0 * x3) * (x2 - 2.0 * x3);
        residuals[3] = sqrt(10.0) * (x1 - x4) * (x1 - x4);
        if (jacobians != NULL && jacobians[0] != NULL)//J = 4x4 f only for jacobian[0]
        {
            //d(f1)/dx1234
            jacobians[0][0] = 1.0;
            jacobians[0][1] = 10.0;
            jacobians[0][2] = 0.0;
            jacobians[0][3] = 0.0;
            //d(f2)
            jacobians[0][4] = 0.0;
            jacobians[0][5] = 0.0;
            jacobians[0][6] = sqrt(5.0);
            jacobians[0][7] = -sqrt(5.0);
            //d(f3)
            jacobians[0][8] = 0.0;
            jacobians[0][9] = 2.0 * x2;
            jacobians[0][10] = -4.0 * x2;
            jacobians[0][11] = 0.0;
            //d(f4)
            jacobians[0][12] = 2.0 * sqrt(10.0);
            jacobians[0][13] = 0.0;
            jacobians[0][14] = 0.0;
            jacobians[0][15] = -2.0 * sqrt(10.0);
        }
        return true;
    }
};

//std::vector<int> sizeParaBlock = parameter_block_sizes();
//std::cout<< "sizeParaBlock = "<<sizeParaBlock.size()<<std::endl;
//int numR = num_residuals();
//std::cout<< "num of residuals = "<<numR<<std::endl;

int main(int argc, char **argv)
{
    GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
    google::InitGoogleLogging(argv[0]);
    double x1 = 3.0;
    double x2 = -1.0;
    double x3 = 0.0;
    double x4 = 1.0;
//    Problem problem;
//    // Add residual terms to the problem using the using the autodiff
//    // wrapper to get the derivatives automatically. The parameters, x1 through
//    // x4, are modified in place.
//    problem.AddResidualBlock(
//            new AutoDiffCostFunction<F1, 1, 1, 1>(new F1), NULL, &x1, &x2);
//    problem.AddResidualBlock(
//            new AutoDiffCostFunction<F2, 1, 1, 1>(new F2), NULL, &x3, &x4);
//    problem.AddResidualBlock(
//            new AutoDiffCostFunction<F3, 1, 1, 1>(new F3), NULL, &x2, &x3);
//    problem.AddResidualBlock(
//            new AutoDiffCostFunction<F4, 1, 1, 1>(new F4), NULL, &x1, &x4);
//    Solver::Options options;
//    LOG_IF(
//            FATAL,
//            !ceres::StringToMinimizerType(FLAGS_minimizer, &options.minimizer_type))
//    << "Invalid minimizer: " << FLAGS_minimizer
//    << ", valid options are: trust_region and line_search.";
//    options.max_num_iterations = 100;
//    options.linear_solver_type = ceres::DENSE_QR;
//    options.minimizer_progress_to_stdout = true;
//    // clang-format off
//    std::cout << "Initial x1 = " << x1
//              << ", x2 = " << x2
//              << ", x3 = " << x3
//              << ", x4 = " << x4
//              << "\n";
//    // clang-format on
//    // Run the solver!
//    Solver::Summary summary;
//    Solve(options, &problem, &summary);
//    std::cout << summary.FullReport() << "\n";
//    // clang-format off
//    std::cout << "Final x1 = " << x1
//              << ", x2 = " << x2
//              << ", x3 = " << x3
//              << ", x4 = " << x4
//              << "\n";
//    // clang-format on

    //ANALY
    Problem problemAnaly;
    std::vector<double *> para_blocks;
    double x_1234[4] = {3, -1, 0, 1};
    problemAnaly.AddResidualBlock(new PowellAnalyticAll, NULL, x_1234);
    Solver::Options options1;
    options1.max_num_iterations = 100;
    options1.linear_solver_type = ceres::DENSE_QR;
    options1.minimizer_progress_to_stdout = true;
    Solver::Summary summary1;
    Solve(options1, &problemAnaly, &summary1);
    std::cout << summary1.FullReport() << "\n";
    std::cout << "Final x1 = " << x_1234[0]
              << ", x2 = " << x_1234[1]
              << ", x3 = " << x_1234[2]
              << ", x4 = " << x_1234[3]
              << "\n";
    return 0;
}